#include "Utils.hpp"
#include "highgui.h"
#include "cv.h"
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <FL/Fl_File_Chooser.H>

Utils::Utils() {};

Utils::~Utils() {};

void Utils::drawRect( IplImage* img, int x1, int y1, int x2, int y2) {
   cvRectangle (
      img,
      cvPoint(x1,y1),
      cvPoint(x2,y2),
      cvScalar(0x00,0x00,0xff)    /* red */
   );
}

Triangle* Utils::drawAndCreateTriangle( IplImage* img, int midX1, int midY1, int x2, int y2, int x3, int y3) {
	double sideLength = sqrt((pow(y3-y2,2) + pow(x3-x2,2)));
	double longDisFromMiddle = (0.5/*sin 30*/ * sideLength) / (sqrt(3)/2);
	CvPoint topPoint = cvPoint(midX1,midY1-longDisFromMiddle);
	double shortDisFromMiddle = sqrt(pow(longDisFromMiddle,2) - pow(sideLength/2,2));
	CvPoint leftPoint = cvPoint(midX1-sideLength/2,midY1+shortDisFromMiddle);
	CvPoint rightPoint = cvPoint(midX1+sideLength/2,midY1+shortDisFromMiddle);

	cvLine(img,topPoint,leftPoint,cvScalar(0x00,0x00,0xff),3,1,0);
	cvLine(img,topPoint,rightPoint,cvScalar(0x00,0x00,0xff),3,1,0);
	cvLine(img,rightPoint,leftPoint,cvScalar(0x00,0x00,0xff),3,1,0);

	return new Triangle(topPoint.x,topPoint.y,leftPoint.x,leftPoint.y,rightPoint.x,rightPoint.y);
}

Circle* Utils::drawAndCreateCircle(  IplImage* img, int midX, int midY, int periX, int periY) {
	double radius = sqrt((pow(midY-periY,2) + pow(midX-periX,2)));
	cvCircle(img,
			cvPoint(midX,midY),
			radius,
			cvScalar(0x00,0x00,0xff),3,1,0);
	return new Circle(midX,midY,radius);
}

void Utils::createSharpFilter(CvMat* dftFilter, string filterKind,Shape* chosenShape) {

	double content;

	if (filterKind.compare("low pass")==0) {
		cvSet(dftFilter, cvScalarAll(0.0), NULL);
		content = 1.0;
	} else { //"high pass"
		cvSet(dftFilter, cvScalarAll(1.0), NULL);
		content = 0.0;
	}

	if (chosenShape->isRectangle()) {
		Rectangle* rect = (Rectangle*)chosenShape;
		cvRectangle(dftFilter,cvPoint(rect->getLeftUpX(), rect->getLeftUpY()),
							  cvPoint(rect->getRightBotX(), rect->getRightBotY()),
							  cvScalarAll(content),-1,0,0);

	} else if (chosenShape->isTriangle()) {
		Triangle* tri = (Triangle*)chosenShape;
		CvPoint* pa = new CvPoint[3];

		pa[0] = cvPoint(tri->getX1Top(),tri->getY1Top());
		pa[1] = cvPoint(tri->getX2Left(), tri->getY2Left());
		pa[2] = cvPoint(tri->getX3Right(), tri->getY3Right());
		int size = 3;
		cvFillConvexPoly(dftFilter, pa, size,cvScalarAll(content), CV_AA, 0 );
		cvPolyLine(dftFilter,&pa,&size,1,1,cvScalarAll(content),-1,0,0);
		delete pa;

	} else { //chosenShape is a circle
		Circle* circ = (Circle*)chosenShape;
		cvCircle(dftFilter, cvPoint(circ->getX(), circ->getY()),
				circ->getRadius(), cvScalarAll(content), -1 , 0 ,0);
	}

}

void Utils::createSmoothFilter(/*IplImage* img*/CvMat* dftFilter, string filterKind,Shape* chosenShape){
	createSharpFilter(dftFilter,filterKind,chosenShape);
	cvSmooth(dftFilter, dftFilter,CV_GAUSSIAN , 9, 9);
//	cvSmooth(img, img,/*CV_BLUR*/CV_GAUSSIAN , 9, 9);
}



IplImage* Utils::fft(IplImage* img ,int chosenFilterType, int chosenFilter, Shape* actualChosenShape) {

	IplImage * realInput;
    IplImage * imaginaryInput;
    IplImage * complexInput;
    int dft_M, dft_N;
    CvMat* dft_A;
    CvMat* dftFilter;
    IplImage * image_Re;
    IplImage * image_Im;
    double m, M;

    realInput = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1);
    imaginaryInput = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1);
    complexInput = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 2);

    dft_M = cvGetOptimalDFTSize( img->height - 1 );
    dft_N = cvGetOptimalDFTSize( img->width - 1 );

    dft_A = cvCreateMat( dft_M, dft_N, CV_32FC2 );
    dftFilter = cvCreateMat( dft_M, dft_N, CV_32FC2 );

    CvMat tmp;

    image_Re = cvCreateImage( cvSize(dft_N, dft_M), IPL_DEPTH_32F, 1);
    image_Im = cvCreateImage( cvSize(dft_N, dft_M), IPL_DEPTH_32F, 1);

    // define grayscale image

    IplImage* grayImg = cvCreateImage(cvSize(img->width,img->height), img->depth, 1);
   	grayImg->origin = img->origin;


   	if (img->nChannels > 1){
   		cvCvtColor(img, grayImg, CV_BGR2GRAY);
   	} else {
   		grayImg = img;
   	}

   	// convert grayscale image to real part of DTF input

	cvScale(grayImg, realInput, 1.0, 0.0);
	cvZero(imaginaryInput);
	cvMerge(realInput, imaginaryInput, NULL, NULL, complexInput);

	// copy A to dft_A and pad dft_A with zeros

	cvGetSubRect( dft_A, &tmp, cvRect(0,0, grayImg->width,grayImg->height));
	cvCopy( complexInput, &tmp, NULL );
	cvGetSubRect( dft_A, &tmp, cvRect(img->width,0, dft_A->cols - grayImg->width, grayImg->height));


	if ((dft_A->cols - grayImg->width) > 0) {
		cvZero( &tmp );
	 }

	 // set up filter (first channel is real part / second is imaginary

	switch (chosenFilterType) {
		case 1: {
			createSharpFilter(dftFilter, chosenFilter==1? "low pass" : "high pass", actualChosenShape);
		}
		break;
		case 2: {
			/*createSmoothFilter(img,chosenFilter==1? "low pass" : "high pass", actualChosenShape);
			return img;*/
			createSmoothFilter(dftFilter, chosenFilter==1? "low pass" : "high pass", actualChosenShape);
		}
		break;
	}

	// no need to pad bottom part of dft_A with zeros because of
	// use nonzero_rows parameter in cvDFT() call below

	cvDFT( dft_A, dft_A, CV_DXT_FORWARD, complexInput->height );
	cvShiftDFT( dft_A, dft_A );

	// apply filter

	cvMulSpectrums( dft_A, dftFilter, dft_A, 0);
	cvShiftDFT( dft_A, dft_A );

    // no need to pad bottom part of dft_A with zeros because of
    // use nonzero_rows parameter in cvDFT() call below

	// invert dft

	cvDFT( dft_A, dft_A, CV_DXT_INVERSE, complexInput->height );
	cvGetSubRect( dft_A, &tmp, cvRect(0,0, grayImg->width,grayImg->height));

	// Split Fourier in real and imaginary parts
	cvSplit( dft_A, image_Re, image_Im, 0, 0 );

	// scale image for display
	cvMinMaxLoc(image_Re, &m, &M, NULL, NULL, NULL);
	cvScale(image_Re, image_Re, 1.0/(M-m), 1.0*(-m)/(M-m));

	image_Re->origin = grayImg->origin;
	//converts the 32 bit gray scale filtered image into an 8 bit gray scale image
	IplImage* image8bit = cvCreateImage( cvSize(dft_N, dft_M), IPL_DEPTH_8U, 1);
	cvConvertScale(image_Re, image8bit, 256);

    return image8bit;
}


void Utils::cvShiftDFT(CvArr * src_arr, CvArr * dst_arr )
{
    CvMat * tmp;
    CvMat q1stub, q2stub;
    CvMat q3stub, q4stub;
    CvMat d1stub, d2stub;
    CvMat d3stub, d4stub;
    CvMat * q1, * q2, * q3, * q4;
    CvMat * d1, * d2, * d3, * d4;

    CvSize size = cvGetSize(src_arr);
    CvSize dst_size = cvGetSize(dst_arr);
    int cx, cy;

    if(dst_size.width != size.width ||
       dst_size.height != size.height){
        cvError( CV_StsUnmatchedSizes, "cvShiftDFT", "Source and Destination arrays must have equal sizes", __FILE__, __LINE__ );
    }

    if(src_arr==dst_arr){
        tmp = cvCreateMat(size.height/2, size.width/2, cvGetElemType(src_arr));
    }

    cx = size.width/2;
    cy = size.height/2; // image center

    q1 = cvGetSubRect( src_arr, &q1stub, cvRect(0,0,cx, cy) );
    q2 = cvGetSubRect( src_arr, &q2stub, cvRect(cx,0,cx,cy) );
    q3 = cvGetSubRect( src_arr, &q3stub, cvRect(cx,cy,cx,cy) );
    q4 = cvGetSubRect( src_arr, &q4stub, cvRect(0,cy,cx,cy) );
    d1 = cvGetSubRect( dst_arr, &d1stub, cvRect(0,0,cx,cy) );
    d2 = cvGetSubRect( dst_arr, &d2stub, cvRect(cx,0,cx,cy) );
    d3 = cvGetSubRect( dst_arr, &d3stub, cvRect(cx,cy,cx,cy) );
    d4 = cvGetSubRect( dst_arr, &d4stub, cvRect(0,cy,cx,cy) );

    if(src_arr!=dst_arr){
        if( !CV_ARE_TYPES_EQ( q1, d1 )){
            cvError( CV_StsUnmatchedFormats, "cvShiftDFT", "Source and Destination arrays must have the same format", __FILE__, __LINE__ );
        }
        cvCopy(q3, d1, 0);
        cvCopy(q4, d2, 0);
        cvCopy(q1, d3, 0);
        cvCopy(q2, d4, 0);
    }
    else{
        cvCopy(q3, tmp, 0);
        cvCopy(q1, q3, 0);
        cvCopy(tmp, q1, 0);
        cvCopy(q4, tmp, 0);
        cvCopy(q2, q4, 0);
        cvCopy(tmp, q2, 0);
    }
}
